
#include "wavefront.h"

NodeStack::NodeStack()
      :nodeid(0), penalty(0) { 
   
}

NodeStack::NodeStack(uint id, float p)
      :nodeid(id), penalty(p) {
   
}

Node::Node() {
   
   wfront = 0;
   active_indicator = false;
   penalty = cost = 0;
}

Node::Node(Vertex_handle vh, Halfedge_handle parent) {
   
   h = vh;
   
   hprev = h->halfedge()->next();
   hnext = h->halfedge()->opposite()->next();
   hparent = parent;
   
   //~ nodeant = ;
   //~ wfront = ;
   //~ idseed = ;  
   //~ cost = ;
   //~ penalty = ;
   active_indicator = false;
}

Node::~Node() {
   
}

void Node::print() {
   
   cout
   <<" "<<nodeant
   <<" "<<nodepost
   <<" "<<wfront
   <<" "<<idseed
   <<" "<<cost
   <<" "<<penalty
   <<" "<<active_indicator
   <<endl;
}

OpenList::OpenList() {
   
   nodes.clear();
}

OpenList::~OpenList() {
   
   nodes.clear();
   //~ queue.clear();
}

void OpenList::print() {
   
   for (uint i=0; i<nodes.size(); ++i) {
      nodes[i].print();
   }
}

void OpenList::print(Mesh *model) {
   
   cout<<"::nodes.size(): "<<nodes.size()<<endl;
   for (uint i=0; i<nodes.size(); ++i) {
      Node &node = nodes[i];
      const Point3 &p = node.h->point();
      cout<<"("<<i<<") "<<node.h->id()<<" : "<<node.hparent->opposite()->vertex()->id()<<" <> "<<" ("<<p[0]<<" "<<p[1]<<" "<<p[2]<<")"<<endl;
      node.print();
   }
   
   priority_queue<NodeStack, vector<NodeStack>, compareNodeStack>   queue_tmp(queue);
   cout<<"~~~("<<queue_tmp.size()<<") ";
   while (not queue_tmp.empty()) {
      NodeStack ns = queue_tmp.top();
      queue_tmp.pop();
      cout<<" [ "<<ns.nodeid<<" : "<<ns.penalty<<"] ";
   }
   cout<<"~~~"<<endl;
}

void OpenList::print_queue() {
   
   priority_queue<NodeStack, vector<NodeStack>, compareNodeStack>   queue_tmp(queue);
   cout<<"~~~("<<queue_tmp.size()<<") ";
   while (not queue_tmp.empty()) {
      NodeStack ns = queue_tmp.top();
      queue_tmp.pop();
      cout<<" [ "<<ns.nodeid<<" : "<<ns.penalty<<"] ";
   }
   cout<<"~~~"<<endl;
}

Vector3 OpenList::unitary(Mesh *model, Halfedge_handle h) {
   
   const Point3 &p = h->vertex()->point();
   const Point3 &q = h->opposite()->vertex()->point();
   Vector3 v = p-q;
   return v/sqrt(v*v);
}

OpenList::OpenList(list<Vertex_handle> &ring, int idseed, Mesh *model, WaveFront &wf) {
   
   wf.idseed = idseed;
   
   list<Vertex_handle>::iterator it = ring.begin();
   while (it != ring.end()) {
      Vertex_handle v = *it;
      
      Node node;
      v->last_seed_reaching_vertex(idseed);
      
      node.h = v;
      /// ??????????????????????????????????????????
      Halfedge_handle hh = v->halfedge()->opposite();
      node.hprev = hh->opposite()->next()->opposite();
      node.hnext = hh->prev()->opposite();
      node.hparent = hh->opposite();
      node.nodepost = nodes.size() + 1;
      node.nodeant = nodes.size() - 1;
      node.wfront = idseed;
      node.idseed = idseed;
      
      // lenght of hh
      //~ float *phh = &model->vpositions[hh->vertex()->id()];
      //~ float *qhh = &model->vpositions[hh->opposite()->vertex()->id()];
      //~ const Vector3 l(phh[0]-qhh[0],phh[1]-qhh[1],phh[2]-qhh[2]);
      //~ node.cost = sqrt(l*l);
      node.cost = 0; /// ???????????????????????
      
      if (v->idclosestSeed() < 0) {
         node.active_indicator = true;
         v->idclosestSeed(idseed);
      }
      else if (v->idclosestSeed() != idseed) {
         node.active_indicator = false;
         wf.activity = false;
      }
      
      nodes.push_back(node);
      
      const Point3 &p = node.h->point();
      const Vector3 &pn = node.h->normal();
      wf.sumVertices = wf.sumVertices + Vector3(p[0],p[1],p[2]);
      wf.sumEdges = wf.sumEdges + unitary(model, node.hparent);
      wf.sumNormals = wf.sumNormals + pn;
      
      ++it;
   }
   
   nodes[0].nodeant = nodes.size() - 1;
   nodes[nodes.size() - 1].nodepost = 0;
   
   wf.firstNode = 0; // o primeiro node
   wf.nNodes = nodes.size();
   wf.dpEdges = wf.sumEdges/(float)wf.nNodes;
   wf.dpNormals = wf.sumNormals/(float)wf.nNodes;
   wf.masscenter = wf.sumVertices/(float)wf.nNodes;
   
   
   /// computing penalties for each Node and the ray2 value for the wavefront
   const Vector3 &mc = wf.masscenter;
   for (uint i=0; i< nodes.size(); ++i) {
      Node &node = nodes[i];
      
      const Point3 &p = node.h->point();
      const Vector3 ray(p[0]-mc[0],p[1]-mc[1],p[2]-mc[2]);
      if (ray*ray > wf.ray2)
         wf.ray2 = ray*ray;
      
      const Vector3 uhp = unitary(model, node.hparent);
      const Vector3 uha = unitary(model, node.hprev);
      const Vector3 uhn = unitary(model, node.hnext);
      node.penalty = compute_penalty(model, node.cost, wf, node.h, uhp, uha, uhn);
      //~ node.penalty = 0; /// ?????????????????
      
      NodeStack ns(i, node.penalty);
      queue.push(ns);
   }
   
}

OpenList::OpenList(Vertex_handle seed, int idseed, Mesh *model, WaveFront &wf) {
   
   cout<<"OpenList::OpenList"<<endl;
   seed->last_seed_reaching_vertex(idseed);
   wf.idseed = idseed;
   
   Halfedge_handle hh = seed->halfedge(), hhbegin = hh;
   do {
      Vertex_handle v = hh->opposite()->vertex();
      
      //~ Node n(v, hh, idseed, active_indicator, cost);
      Node node;
      
      v->last_seed_reaching_vertex(idseed);
      
      node.h = v;
      node.hprev = hh->opposite()->next()->opposite();
      node.hnext = hh->prev()->opposite();
      node.hparent = hh->opposite();
      node.nodepost = nodes.size() + 1;
      node.nodeant = nodes.size() - 1;
      node.wfront = idseed;
      node.idseed = idseed;
      
      // lenght of hh
      const Point3 &phh = hh->vertex()->point();
      const Point3 &qhh = hh->opposite()->vertex()->point();
      const Vector3 l = phh - qhh;
      node.cost = sqrt(l*l);
      
      if (v->idclosestSeed() < 0) {
         node.active_indicator = true;
         v->idclosestSeed(idseed);
      }
      else if (v->idclosestSeed() != idseed) {
         node.active_indicator = false;
         wf.activity = false;
      }
      
      //~ cout<<"opa node"<<endl;
      //~ n.print();
      nodes.push_back(node);
      
      const Point3 &p = node.h->point();
      const Vector3 &pn = node.h->normal();
      wf.sumVertices = wf.sumVertices + Vector3(p[0],p[1],p[2]);
      wf.sumEdges = wf.sumEdges + unitary(model, node.hparent);
      wf.sumNormals = wf.sumNormals + pn;
      
      //~ cout<<"vertice: "<<Vector3(p[0],p[1],p[2])<<endl;
      //~ cout<<" ... "<<endl;
      //~ wf.print();
      
      hh = hh->next()->opposite(); // 
   } while(hh != hhbegin);
   
   nodes[0].nodeant = nodes.size() - 1;
   nodes[nodes.size() - 1].nodepost = 0;
   
   wf.firstNode = 0; // o primeiro node
   wf.nNodes = nodes.size();
   wf.dpEdges = wf.sumEdges/(float)wf.nNodes;
   wf.dpNormals = wf.sumNormals/(float)wf.nNodes;
   wf.masscenter = wf.sumVertices/(float)wf.nNodes;
   
   /// computing penalties for each Node and the ray2 value for the wavefront
   const Vector3 &mc = wf.masscenter;
   for (uint i=0; i< nodes.size(); ++i) {
      Node &node = nodes[i];
      
      const Point3 &p = node.h->point();
      const Vector3 ray(p[0]-mc[0],p[1]-mc[1],p[2]-mc[2]);
      if (ray*ray > wf.ray2)
         wf.ray2 = ray*ray;
      
      const Vector3 uhp = unitary(model, node.hparent);
      const Vector3 uha = unitary(model, node.hprev);
      const Vector3 uhn = unitary(model, node.hnext);
      node.penalty = compute_penalty(model, node.cost, wf, node.h, uhp, uha, uhn);
      
      NodeStack ns(i, node.penalty);
      queue.push(ns);
   }
   
   //~ cout<<" no final ... "<<endl;
   //~ wf.print();
}

void OpenList::pushback(Node& node) {
   
   
   // we must get the correct id position in the nodes list
   NodeStack ns(nodes.size(), node.penalty);
   nodes.push_back(node);
   queue.push(ns);
}

float OpenList::compute_penalty(Mesh *model, float cost, WaveFront &wf, Vertex_handle m, 
                                const Vector3 &uhp, const Vector3 &uha, const Vector3 &uhn) {
   
   float penalty;
   float weightcost=1, weightedges=0, weightnormals=0; float lowerbound = 0.05;
   
   penalty = weightcost*cost;
   float norma = sqrt(wf.dpEdges*wf.dpEdges);
   const Point3 &p = m->point();
   Vector3 v(p[0],p[1],p[2]);
   Vector3 dif = v - wf.masscenter;
   
   if (norma > lowerbound) {
      penalty += (dif*wf.dpEdges/norma)*weightedges;
   }
   
   norma = sqrt(wf.dpNormals*wf.dpNormals);
   if (norma > lowerbound) {
      float norma2=dif*dif;
      if (norma2 < 10e-8) return penalty;
      
      dif = dif/sqrt(dif*dif);
      Vector3 vecprod = cross_product(uhp,uha+uhn);
      if (vecprod*vecprod < 10e-8) return penalty;
      
      vecprod = vecprod/sqrt(vecprod*vecprod);
      float cos = vecprod*dif;
      float cos2 = cos*cos;
      if (cos2 < 10e-8) return penalty;
      penalty += weightnormals*sqrt(norma2*(1-cos2)/cos2);
   }
   
   return penalty;
}

WaveFront::WaveFront() {
   
   nNodes = 0;
   sumVertices = Vector3(0,0,0);
   sumEdges = Vector3(0,0,0);
   sumNormals = Vector3(0,0,0);
   dpEdges = Vector3(0,0,0);
   dpNormals = Vector3(0,0,0);
   masscenter = Vector3(0,0,0);
   
   ray2 = 0;
   activity = true;
   firstNode = -1;
   
   idseed = -1;
   
   band = -1;
   oldestnode = -1;
}

WaveFront::WaveFront(int idSeed) {
   
   nNodes = 0;
   sumVertices = Vector3(0,0,0);
   sumEdges = Vector3(0,0,0);
   sumNormals = Vector3(0,0,0);
   dpEdges = Vector3(0,0,0);
   dpNormals = Vector3(0,0,0);
   masscenter = Vector3(0,0,0);
   
   ray2 = 0;
   activity = true;
   firstNode = -1;
   
   idseed = idSeed;
   band = -1;
   oldestnode = -1;
}

WaveFront::~WaveFront() {
   
}

void WaveFront::print() {
   
   cout
   <<" "<<nNodes
   <<" "<<sumVertices
   <<" "<<sumEdges
   <<" "<<sumNormals
   <<" "<<dpEdges
   <<" "<<dpNormals
   <<" "<<masscenter
   <<" "<<ray2
   <<" "<<activity
   <<" "<<idseed
   <<" "<<firstNode
   <<" "<<band
   <<" "<<oldestnode
   <<endl;
}
